#include "../inc/hw_types.h"
#include "uart.h"
#include "OS.h"
#include "IR.h"
#include "FIFO.h"
#include "rit128x96x4.h"
#include "adc_c.h"
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include <math.h>

_tFIFO Distance;
unsigned long filterArray[4] = {0,0,0,0};

void IR_Init(void)
{
  Distance = FIFO_CreateFifo(32, sizeof(unsigned long));
}

void IRDataCollect(OS_TCB* this)
{
  char buf[30];
  unsigned long dataIR1;
  unsigned long dataIR2;
  unsigned long dataIR3;
  unsigned long dataSend; 
  int i = 0;
  int counter = 0;
  int device = 0;
  while(1){
    int whole;
	int decimal;
    dataIR1 = ADC_In();
    //dataIR2 = ADC_In(1);
    //dataIR2 = ADC_In(2);
	dataIR1 = filter(dataIR1);
	if(dataIR1 >= 1023)
	{
	  dataSend = 1;
	}
	else
	{
	  dataSend = (int)((17570*pow((double)dataIR1, -1.272))*100);
	}
	whole = (int)(dataSend/100);
	decimal = (dataSend%100);
	FIFO_PutSingle(Distance, &dataSend);
	//if((i>= 50))
	//{
	  sprintf(buf,"Dist (in) %d.%d  ",(int)whole, (int)decimal);
	  if((device != 1) || (counter != 4)){
	  oLED_Message(device,counter,buf);
	  }
	  counter++;
	  if((counter == 4) && (device == 0))
	  {
	    counter = 0;
		device = 1;
	  }
	  i = 0;
	  //sprintf(buf,"Dist ADC %d  ",(int)dataIR1);
	  //oLED_Message(1, 0, buf);
	//}
	i++;
	OS_Sleep(this,30); //Need this thread to sleep for 30 ms
  }
}

unsigned long filter(unsigned long data)
{
  int a;
  unsigned long filteredData;
  for( a = 2; a>=0; a--)
  {
    filterArray[a+1] = filterArray[a];
  }
  filterArray[0] = data;
  filteredData = ((filterArray[0]+filterArray[3])>>1);
  return filteredData;
}








